#include<stdio.h>
#include <ppf.h>
//#include<vector>
//#include <unordered_map>
#include<Mesh/Mesh.h>
#include<Mesh/libcell_iterator.h>
#include  "Arcroll.h"


#define quote lib_cell_quote




typedef struct MY_TestViewer{
    int i;
    Ppf_Out out;
    Viewer_Something* vs;
    Viewer_Opengl_Interpreter*voi;
    float* data;
    void *prop;

} MY_TestViewer;
static inline void mat_mult_array(LB_Matrix* mat, double* p, double* output)
{
    memset(output, 0, sizeof(double) * 3);
    double* data = (double*)(mat->data);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            output[i] += p[j] * data[i * 4 + j];
        }
        output[i] += data[i * 4 + 3];
    }
}
static inline void my_testviewer_init(MY_TestViewer* mtv)
{
    mtv->i=0;
    
    mtv->vs=NULL;
    mtv->voi=NULL;
    mtv->data=NULL;
    mtv->prop=NULL;
}

void my_test_key_fun(Viewer_Intera*vi)
{
    Interactor_GlobalInfo* g_info=vi->g_info;
    MY_TestViewer*mvt=(MY_TestViewer*)(vi->representation);
    Ppf_Out out =mvt->out;

    if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_F)
    {   
        Viewer_Points*vp=(Viewer_Points*)(mvt->vs->evolution);
        LB_Matrix*mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_double(mat,4,4);
        double temp1[3]={0},temp2[3]={0};
        // printf("fit num:%d votes:%d\n", ppf->poses_result[mvt->i].fitting_numbers, 
        //     ppf->poses_result[mvt->i].votes);
        memmove(mat->data,out.mats[mvt->i],sizeof(double)*16);

        for(int i=0;i<vp->Data_rows;i++)
        {

            temp1[0]=mvt->data[i*3+0];temp1[1]=mvt->data[i*3+1];temp1[2]=mvt->data[i*3+2];
            mat_mult_array(mat,temp1,temp2);
            vp->Data[i*3+0]=temp2[0];vp->Data[i*3+1]=temp2[1]; vp->Data[i*3+2]=temp2[2];
        }
        lb_matrix_free(mat);

        mvt->voi->update_data(mvt->voi);
        mvt->i++;
    }
}

void my_test_viewer(Viewer_World* vw,Viewer_Opengl_Interpreter*voi,Ppf_Out out ,float*data,int size)
{
    MY_TestViewer* mvt=(MY_TestViewer*)malloc(sizeof(MY_TestViewer));
    my_testviewer_init(mvt);
    mvt->out=out;
    mvt->voi=voi;
    mvt->data=data;
    Node* n=vw->create_something(vw,"Points");
    auto vs=(Viewer_Something*)(n->value);
    //vs->disappear = 1;
    auto vp=(Viewer_Points*)(vs->evolution);
    vp->Data_rows=size;
    vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
    memmove(vp->Data,data,sizeof(float)*3*vp->Data_rows);
    vp->pointsize=8.0;
    vp->set_color(vp,1,0,0,1);
    double temp1[3]={0},temp2[3]={0};
   /* ppf->poses_result[0].pose->print_self(ppf->poses_result[0].pose);
    printf("delt:%lf\n", *((double*)(ppf->poses_result[0].pose->det(ppf->poses_result[0].pose))));
    */
    LB_Matrix*mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(mat,4,4);

    memmove(mat->data,out.mats[0],sizeof(double)*16);
    for(int i=0;i<vp->Data_rows;i++)
    {
        temp1[0]=data[i*3+0];temp1[1]=data[i*3+1];temp1[2]=data[i*3+2];
        mat_mult_array(mat,temp1,temp2);
        vp->Data[i*3+0]=temp2[0];vp->Data[i*3+1]=temp2[1]; vp->Data[i*3+2]=temp2[2];

    }
    free_node(n);
    lb_matrix_free(mat);
    mvt->vs=vs;
    n=vw->create_something(vw,"Intera");
    vs=(Viewer_Something*)(n->value);
    auto vi=(Viewer_Intera*)(vs->evolution);
    vi->representation=mvt;
    vi->key_callback=my_test_key_fun;
    free_node(n);
}

void my_test_ppf()
{
    //******************
    Viewer_World_Manager vwm;
    viewer_world_manager_init(&vwm);
    Viewer_World* vw=vwm.create_world(&vwm,"中原动力位姿标注");
    //vw->background_color[0]=244;    vw->background_color[1]=245;vw->background_color[2]=245;    

    Viewer_Opengl_Interpreter voi;
    Viewer_Arcroll* va= add_default_somethings(vw);
    //**************************
    clock_t t = clock();

    int nums[2];
    char file[30]="111807.pcd";
    double ** array_scene=my_read_arr(file,&(nums[0]));
    char file1[30]="cylinder1.pcd";

    double** array_model=my_read_arr(file1,&(nums[1]));



    Ppf_Recommand_Params prp;
    ppf_recommand_params_init(&prp);
    prp.model_sample=0.018;//0.018
    prp.scene_sample=0.018;//0.018
    //prp.is_inverse_normal = 0;
    prp.dis_disc_step = 0.014;
    prp.pose_cluster_threold = 0.06;
    
    void* hash_map;double ad_fac;
    void*m2= compute_model_feature(array_model,nums[1] , &prp,&hash_map,&ad_fac );

    Mesh * model=(Mesh*)m2;



//***********************

    Node* n= vw->create_something(vw,"Points");
    auto vs=(Viewer_Something*)(n->value);
    //vs->disappear = 1;
    auto vp=(Viewer_Points*)(vs->evolution);
    vp->Data_rows=model->num_v(model);
    vp->Data=(float*)malloc(sizeof(float)*vp->Data_rows*3);
    auto vit=model->v_begin(model);
    for(int i=0;i<vp->Data_rows;i++)
    {
        vp->Data[i*3+0]=quote(vit)->point[0];
        vp->Data[i*3+1]=quote(vit)->point[1];
        vp->Data[i*3+2]=quote(vit)->point[2];
        vit++;
    }
    vp->pointsize=10.0;
    vp->set_color(vp,1,0,0,1);
    free_node(n);

    float* data=(float*)malloc(sizeof(float)*3*model->num_v(model));
    vit=model->v_begin(model);
    for(int i=0;i<model->num_v(model);i++)
    {
        data[i*3+0]=quote(vit)->point[0];
        data[i*3+1]=quote(vit)->point[1];
        data[i*3+2]=quote(vit)->point[2];
        vit++;
    }


    n=vw->create_something(vw,"Edges");
    vs=(Viewer_Something*)(n->value);
    auto ve=(Viewer_Edges*)(vs->evolution);
    ve->Data_index_rows=model->num_v(model);
    ve->Data_rows=ve->Data_index_rows*2;
    ve->Data=(float*)malloc(sizeof(float)*3*ve->Data_rows);
    ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
    ve->color_rows=ve->Data_index_rows;
    ve->set_color(ve,1.0,0.0,0.0,1.0);
    vit=model->v_begin(model);
    double fac1=0.05;
    for(int i=0;i<ve->Data_index_rows;i++)
    {
        ve->Data[(i*2+0)*3+0]=quote(vit)->point[0]; ve->Data[(i*2+0)*3+1]=quote(vit)->point[1];ve->Data[(i*2+0)*3+2]=quote(vit)->point[2];
        double*normal=(double*)(quote(vit)->prop);
        ve->Data[(i*2+1)*3+0]=quote(vit)->point[0]+fac1*normal[0]; ve->Data[(i*2+1)*3+1]=quote(vit)->point[1]+ fac1*normal[1];ve->Data[(i*2+1)*3+2]=quote(vit)->point[2]+fac1*normal[2];
        ve->Data_index[i*2+0]=i*2+0; ve->Data_index[i*2+1]=i*2+1;
        vit++;
    }
    free_node(n);
    
//*********************************

    Ppf_Out out=compute_scene_poses(array_scene, nums[0],m2, &prp, hash_map,ad_fac);

    
//***********************
    n= vw->create_something(vw,"Points");
    vs=(Viewer_Something*)(n->value);
    vp=(Viewer_Points*)(vs->evolution);
    vp->Data_rows=nums[0];

    vp->Data=(float*)malloc(sizeof(float)*vp->Data_rows*3);
   
    for(int i=0;i<vp->Data_rows;i++)
    {
        vp->Data[i*3+0]=array_scene[i][0];
        vp->Data[i*3+1]=array_scene[i][1];
        vp->Data[i*3+2]=array_scene[i][2];
     
    }
    vp->set_color(vp,0,1,0,1);
    vp->pointsize=10.0;
    free_node(n);


    // n=vw->create_something(vw,"Edges");
    // vs=(Viewer_Something*)(n->value);
    // ve=(Viewer_Edges*)(vs->evolution);
    // ve->Data_index_rows=n_m2->num_v(n_m2);
    // ve->Data_rows=ve->Data_index_rows*2;
    // ve->Data=(float*)malloc(sizeof(float)*3*ve->Data_rows);
    // ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
    // ve->color_rows=ve->Data_index_rows;
    // ve->set_color(ve,1.0,1.0,0.0,1.0);
    // vit=n_m2->v_begin(n_m2);
    // for(int i=0;i<ve->Data_index_rows;i++)
    // {
    //     ve->Data[(i*2+0)*3+0]=quote(vit)->point[0]; ve->Data[(i*2+0)*3+1]=quote(vit)->point[1];ve->Data[(i*2+0)*3+2]=quote(vit)->point[2];
    //     double*normal=(double*)(quote(vit)->prop);
    //     ve->Data[(i*2+1)*3+0]=quote(vit)->point[0]+fac1*normal[0]; ve->Data[(i*2+1)*3+1]=quote(vit)->point[1]+ fac1*normal[1];ve->Data[(i*2+1)*3+2]=quote(vit)->point[2]+fac1*normal[2];
    //     ve->Data_index[i*2+0]=i*2+0; ve->Data_index[i*2+1]=i*2+1;
    //     vit++;
    // }
    // free_node(n); 

//*************************

    LB_Matrix* m1 = (LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(m1, 4, 4);
    ((double*)(m1->data))[0 * 4 + 0] = ad_fac;((double*)(m1->data))[1 * 4 + 1] = ad_fac;((double*)(m1->data))[2* 4 + 2] = ad_fac;
    
    LB_Matrix* mat2 = m1->inverse(m1);

    int l =out.l;
    
    for (int i = 0; i < l; i++)
    {
        //Eigen::Matrix4f M;
        LB_Matrix* temp = (LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_double(temp,4,4);
        temp->copy_data(temp, m1);


        LB_Matrix* M =(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_double(M,4,4);
        memmove(M->data,out.mats[i],sizeof(double)*16);

        temp->mult(temp, M); temp->mult(temp, mat2);

        memmove(out.mats[i], temp->data, sizeof(double) * 16); 
        lb_matrix_free(M);
        lb_matrix_free(temp);
    }

    lb_matrix_free(mat2);
    lb_matrix_free(m1); 

    my_test_viewer(vw,&voi,out,data,model->num_v(model));

   

    printf("end time%.5f:%d\n", (double)(clock() - t) / CLOCKS_PER_SEC,0);

    //**********************
    viewer_opengl_interpreter_initn(&voi,&vwm);
    voi.interpreter(&voi);
    viewer_opengl_interpreter_free(&voi);
    viewer_world_manager_free(&vwm);
    //*****************
}


int main()
{
   
    my_test_ppf();
    printf("arcos :%f\n",acos(-0.8));
    printf("endnnnn\n");

	return 0;
}
